
import processing.serial.*;
Serial port; 
Serial port2;

// y - 66537
// x - 101695
// definitions -------------------------------------
int LINE_FEED = '\r'; 

// variables ---------------------------------------
int validData = 0;
int timeout;
int timeoutCount = 0;
String inBuffer;
int xpos, ypos; 
int FOV, FOV0, FOV1, FOVz, FOVz0, FOVz1;
int tl_x, tl_y, tl_z, br_x, br_y, br_z;
//int stepWidth, stepHeight;
float columns, rows, stacks;
int processStep = 0;
boolean advance = true;
int delay_before_pic = 2000;
int delay_after_pic = 3000;
int m = 0;
int n = 0;
int o = 0;
String x_string = "0";
String y_string = "0";


//byte[] rspCharArray = new byte[32];

/////////////////// setup //////////////////////////
void setup(){
  size(1017,665);

  // List all the available serial ports:
  println(Serial.list());
  port = new Serial(this, Serial.list()[0], 9600);
  port2 = new Serial(this, Serial.list()[2], 9600);
  delay(500);
  port.clear();

  xpos = 0;
  ypos = 0;
  FOV = 800;
  tl_x = 0;
  tl_y = 0;
  tl_z = 0;
  br_x = 0;
  br_y = 0;
  br_z = 0;
  
  //stepWidth=0;
  //stepHeight=0;

  rectMode(CENTER);
  strokeCap(SQUARE);
  noFill();
  smooth();
}


///////////////////// draw ///////////////////////////
void draw(){
  background(100,100,100);
  translate(1017,665);
  rotate(PI);

  // draw box that shows area of single image
  rect(xpos, ypos, (FOV/200*4/3), (FOV/200));
  // draw box that shows area of the entire array of images
  rect( (((tl_x - br_x)/2 + br_x)/100), (((tl_y - br_y)/2 + br_y)/100), (tl_x - br_x)/100, (tl_y - br_y)/100 );


  float stepWidth = 0.8 * float(FOV);    // step width and heigh is less than the field of view; accounting for overlap.
  float stepHeight = 0.75 * stepWidth;
  float stepDepth = FOVz;
  int arrayWidth = (tl_x - br_x);
  int arrayHeight = (tl_y - br_y);
  int arrayDepth = (tl_z - br_z);

//  get current cursor position
  y_string = commandReceive("py\r");
  if (y_string != null){
    ypos = int(Float.parseFloat(y_string))/100;
  }
  x_string = commandReceive("px\r");
  if (x_string != null){
    xpos = int(Float.parseFloat(x_string))/100;
  }

  // now we can tell the user how many images they're going to have to take
  if(processStep == 3){    // getting bottom, right most image point
    br_x = int(Float.parseFloat(commandReceive("px\r")));
    br_y = int(Float.parseFloat(commandReceive("py\r")));
    br_z = int(Float.parseFloat(commandReceive("pz\r")));
    columns = (tl_x - br_x)/(stepWidth);
    rows = (tl_y - br_y)/(stepHeight);
    stacks = (tl_z - br_z)/(stepDepth);
    println("columns: "+(int)columns+", rows: "+(int)rows+", stacks: "+(int)stacks+", total pics: "+((int)columns*(int)rows*(int)stacks));
  }

  // now we loop through the array and going to each voxel, taking a picture
  if(processStep == 4){    // loop through array
    //println("step 4");
    //delay(1000);


    // calculate the new points based on cell info etc.
    int set_x = tl_x - (m * (int)stepWidth);
    int set_y = tl_y - (n * (int)stepHeight);
    int set_z = tl_z - (o * (int)stepDepth);

    // write the new points
    if(advance == true) delay(20000);  // even more delay so that the user can 
    port.write("gx "+(set_x)+'\r');
    port.write("gy "+(set_y)+'\r');  
    port.write("gz "+(set_z)+'\r');      

    // take a picture and wait
    if(advance == true) delay(20000);  // extra delay to let the stage get to initial position, possibly far away from final pos. that was just set.
    delay(delay_before_pic);      // if we could replace delay with sudo interrupt, things would look nicer...
    port2.write("picture\r");
    delay(delay_before_pic);


    // update cell position m,n
    println("m: "+m+" "+(set_x/100)+", n: "+n+" "+(set_y/100)+", o: "+o);
    
    o++;
    if( (o)*(int)stepDepth > arrayDepth ){
      n++;
      o=0;
    }
    if( (n)*(int)stepHeight > arrayHeight ){
      m++;
      n=0;
    }
    if( (m)*(int)stepWidth > arrayWidth ){
      processStep++;
      advance = true;
    } 
    
    // draw a box showing the picture just taken
    rect( (set_x/100), (set_y/100), stepWidth/100, stepHeight/100);


  }


  /// prompt
  if(advance == true){
    switch(processStep) {
    case 0:     // setting up field of view
      println("Let's set the field of view");
      println("Align object with top of screen, focus on object and press mouse button");
      advance = false;
      break;
    case 1:     // setting up field of view
      println("Align object with bottom of screen, adjust focus to adjacent focal plane and press mouse button");
      advance = false;
      break;
    case 2:     // setting up top, left most point in image matrix 
      println("Align to top, left, highest point and press mouse button");
      advance = false;
      break;
    case 3:     // setting up bottom, right most point in image matrix
      println("Align to bottom, right, lowest point and press mouse button");
      advance = false;
      delay(1000);
      break;  
    case 4:    // begin looping through the matrix of images
      println("Taking micro-gigapan!");
      delay(10000);
      advance = false;
      break;
    case 5:    // exit
      println("all done!");
      advance = false;
      break;
    }
  }

  /// user input from mouse
  if(mousePressed){
    switch(processStep) {
    case 0:   // setting up field of view, top of frame
      FOV0 = int(Float.parseFloat(commandReceive("py\r")));
      FOVz0 = int(Float.parseFloat(commandReceive("pz\r")));
      processStep++;
      advance = true;
      break;
    case 1:    // setting up field of view, bottom of frame
      FOV1 = int(Float.parseFloat(commandReceive("py\r")));
      FOVz1 = int(Float.parseFloat(commandReceive("pz\r")));
      println("your field of y view is: "+(FOV0-FOV1)+", z is: "+(FOVz0-FOVz1));
      FOV = (FOV0-FOV1);
      FOVz = (FOVz0-FOVz1);
      processStep++;
      advance = true;
      break;
    case 2:   // getting top, left, highest most image point
      tl_x = int(Float.parseFloat(commandReceive("px\r")));
      tl_y = int(Float.parseFloat(commandReceive("py\r")));
      tl_z = int(Float.parseFloat(commandReceive("pz\r")));
      println("top left highest is: "+tl_x+", "+tl_y+", "+tl_z);
      processStep++;
      advance = true;
      break;
    case 3: // getting bottom, right most image point
      br_x = int(Float.parseFloat(commandReceive("px\r")));
      br_y = int(Float.parseFloat(commandReceive("py\r")));
      br_z = int(Float.parseFloat(commandReceive("pz\r")));
      println("bottom right lowest is: "+br_x+", "+br_y+", "+br_z);
      processStep++;
      advance = true;
      break;      

    }  
  }

}






/////////////////// commandReceive() ///////////////////
public String commandReceive(String command) {
  // issue the command
  port.write(command);
  //delay(500);
  // get the response
  validData = 0;
  timeout=0;
  while(validData == 0){
    delay(100);
    timeout += 1;
    if(timeout > 4){
      println("timeout");
      validData = -1;
    }
  }
  if(validData != -1){    // we got a good response, now do something with it
    //println("inBuffer: "+inBuffer);
    return new String(inBuffer);
  }
  return null;
}



//////////////////// serialEvent ///////////////////
void serialEvent(Serial port) {
  inBuffer = port.readStringUntil(LINE_FEED);
  if (inBuffer != null) {
    validData = 1;
  }
}

/*
///////////////////// cross ////////////////////////
 void cross(int x, int y, int dim) {
 strokeWeight(5);
 line((x-(dim/2)), y, (x+(dim/2)), y);
 line(x, (y-(dim/2)), x, (y+(dim/2)));
 
 }
 *//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
